
<!DOCTYPE html>

<html xmlns="http://www.w3.org/1999/xhtml">
  <head>
    <meta charset="utf-8" />
    <title>spatialmath.pose2d &#8212; Spatial Maths package 0.7.0
 documentation</title>
    <link rel="stylesheet" href="../../_static/alabaster.css" type="text/css" />
    <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
    <link rel="stylesheet" type="text/css" href="../../_static/graphviz.css" />
    <script id="documentation_options" data-url_root="../../" src="../../_static/documentation_options.js"></script>
    <script src="../../_static/jquery.js"></script>
    <script src="../../_static/underscore.js"></script>
    <script src="../../_static/doctools.js"></script>
    <script src="../../_static/language_data.js"></script>
    <script async="async" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/latest.js?config=TeX-AMS-MML_HTMLorMML"></script>
    <link rel="index" title="Index" href="../../genindex.html" />
    <link rel="search" title="Search" href="../../search.html" />
   
  <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
  
  
  <meta name="viewport" content="width=device-width, initial-scale=0.9, maximum-scale=0.9" />

  </head><body>
  

    <div class="document">
      <div class="documentwrapper">
        <div class="bodywrapper">
          

          <div class="body" role="main">
            
  <h1>Source code for spatialmath.pose2d</h1><div class="highlight"><pre>
<span></span><span class="ch">#!/usr/bin/env python3</span>
<span class="c1"># -*- coding: utf-8 -*-</span>

<span class="kn">from</span> <span class="nn">collections</span> <span class="kn">import</span> <span class="n">UserList</span>
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="kn">import</span> <span class="nn">math</span>

<span class="kn">from</span> <span class="nn">spatialmath.base</span> <span class="kn">import</span> <span class="n">argcheck</span>
<span class="kn">import</span> <span class="nn">spatialmath.base</span> <span class="k">as</span> <span class="nn">tr</span>
<span class="kn">from</span> <span class="nn">spatialmath</span> <span class="kn">import</span> <span class="n">super_pose</span> <span class="k">as</span> <span class="n">sp</span>
<span class="kn">import</span> <span class="nn">spatialmath.pose3d</span> <span class="k">as</span> <span class="nn">p3</span>

<span class="c1"># ============================== SO2 =====================================#</span>

<div class="viewcode-block" id="SO2"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SO2">[docs]</a><span class="k">class</span> <span class="nc">SO2</span><span class="p">(</span><span class="n">sp</span><span class="o">.</span><span class="n">SMPose</span><span class="p">):</span>

    <span class="c1"># SO2()  identity matrix</span>
    <span class="c1"># SO2(angle, unit)</span>
    <span class="c1"># SO2( obj )   # deep copy</span>
    <span class="c1"># SO2( np )  # make numpy object</span>
    <span class="c1"># SO2( nplist )  # make from list of numpy objects</span>

    <span class="c1"># constructor needs to take ndarray -&gt; SO2, or list of ndarray -&gt; SO2</span>
<div class="viewcode-block" id="SO2.__init__"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SO2.__init__">[docs]</a>    <span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">arg</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="o">*</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">True</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Construct new SO(2) object</span>

<span class="sd">        :param unit: angular units &#39;deg&#39; or &#39;rad&#39; [default] if applicable</span>
<span class="sd">        :type unit: str, optional</span>
<span class="sd">        :param check: check for valid SO(2) elements if applicable, default to True</span>
<span class="sd">        :type check: bool</span>
<span class="sd">        :return: SO(2) rotation</span>
<span class="sd">        :rtype: SO2 instance</span>

<span class="sd">        - ``SO2()`` is an SO2 instance representing a null rotation -- the identity matrix.</span>
<span class="sd">        - ``SO2(theta)`` is an SO2 instance representing a rotation by ``theta`` radians.  If ``theta`` is array_like</span>
<span class="sd">          `[theta1, theta2, ... thetaN]` then an SO2 instance containing a sequence of N rotations.</span>
<span class="sd">        - ``SO2(theta, unit=&#39;deg&#39;)`` is an SO2 instance representing a rotation by ``theta`` degrees.  If ``theta`` is array_like</span>
<span class="sd">          `[theta1, theta2, ... thetaN]` then an SO2 instance containing a sequence of N rotations.</span>
<span class="sd">        - ``SO2(R)`` is an SO2 instance with rotation described by the SO(2) matrix R which is a 2x2 numpy array.  If ``check``</span>
<span class="sd">          is ``True`` check the matrix belongs to SO(2).</span>
<span class="sd">        - ``SO2([R1, R2, ... RN])`` is an SO2 instance containing a sequence of N rotations, each described by an SO(2) matrix</span>
<span class="sd">          Ri which is a 2x2 numpy array. If ``check`` is ``True`` then check each matrix belongs to SO(2).</span>
<span class="sd">        - ``SO2([X1, X2, ... XN])`` is an SO2 instance containing a sequence of N rotations, where each Xi is an SO2 instance.</span>
<span class="sd">        </span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="nb">super</span><span class="p">()</span><span class="o">.</span><span class="fm">__init__</span><span class="p">()</span>  <span class="c1"># activate the UserList semantics</span>

        <span class="k">if</span> <span class="n">arg</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
            <span class="c1"># empty constructor</span>
            <span class="k">if</span> <span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">)</span> <span class="ow">is</span> <span class="n">SO2</span><span class="p">:</span>
                <span class="bp">self</span><span class="o">.</span><span class="n">data</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">2</span><span class="p">)]</span>
        <span class="k">elif</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">arg</span><span class="p">):</span>
            <span class="c1"># SO2(value)</span>
            <span class="c1"># SO2(list of values)</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">data</span> <span class="o">=</span> <span class="p">[</span><span class="n">tr</span><span class="o">.</span><span class="n">rot2</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">unit</span><span class="p">)</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">arg</span><span class="p">)]</span>

        <span class="k">elif</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">arg</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">)</span> <span class="ow">and</span> <span class="n">arg</span><span class="o">.</span><span class="n">shape</span> <span class="o">==</span> <span class="p">(</span><span class="mi">2</span><span class="p">,</span><span class="mi">2</span><span class="p">):</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">data</span> <span class="o">=</span> <span class="p">[</span><span class="n">arg</span><span class="p">]</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="nb">super</span><span class="p">()</span><span class="o">.</span><span class="n">_arghandler</span><span class="p">(</span><span class="n">arg</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="n">check</span><span class="p">)</span></div>

<div class="viewcode-block" id="SO2.Rand"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SO2.Rand">[docs]</a>    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">Rand</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="o">*</span><span class="p">,</span> <span class="nb">range</span><span class="o">=</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span><span class="p">],</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">,</span> <span class="n">N</span><span class="o">=</span><span class="mi">1</span><span class="p">):</span>

        <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Construct new SO(2) with random rotation</span>

<span class="sd">        :param range: rotation range, defaults to :math:`[0, 2\pi)`.</span>
<span class="sd">        :type range: 2-element array-like, optional</span>
<span class="sd">        :param unit: angular units as &#39;deg or &#39;rad&#39; [default]</span>
<span class="sd">        :type unit: str, optional</span>
<span class="sd">        :param N: number of random rotations, defaults to 1</span>
<span class="sd">        :type N: int</span>
<span class="sd">        :return: SO(2) rotation matrix</span>
<span class="sd">        :rtype: SO2 instance</span>

<span class="sd">        - ``SO2.Rand()`` is a random SO(2) rotation.</span>
<span class="sd">        - ``SO2.Rand([-90, 90], unit=&#39;deg&#39;)`` is a random SO(2) rotation between </span>
<span class="sd">          -90 and +90 degrees.</span>
<span class="sd">        - ``SO2.Rand(N)`` is a sequence of N random rotations.</span>
<span class="sd">        </span>
<span class="sd">        Rotations are uniform over the specified interval.</span>

<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">rand</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">uniform</span><span class="p">(</span><span class="n">low</span><span class="o">=</span><span class="nb">range</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">high</span><span class="o">=</span><span class="nb">range</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">size</span><span class="o">=</span><span class="n">N</span><span class="p">)</span>  <span class="c1"># random values in the range</span>
        <span class="k">return</span> <span class="bp">cls</span><span class="p">([</span><span class="n">tr</span><span class="o">.</span><span class="n">rot2</span><span class="p">(</span><span class="n">x</span><span class="p">)</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getunit</span><span class="p">(</span><span class="n">rand</span><span class="p">,</span> <span class="n">unit</span><span class="p">)])</span></div>
    
<div class="viewcode-block" id="SO2.Exp"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SO2.Exp">[docs]</a>    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">Exp</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">S</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">True</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Construct new SO(2) rotation matrix from so(2) Lie algebra</span>

<span class="sd">        :param S: element of Lie algebra so(2)</span>
<span class="sd">        :type S: numpy ndarray</span>
<span class="sd">        :param check: check that passed matrix is valid so(2), default True</span>
<span class="sd">        :type check: bool</span>
<span class="sd">        :return: SO(2) rotation matrix</span>
<span class="sd">        :rtype: SO2 instance</span>

<span class="sd">        - ``SO2.Exp(S)`` is an SO(2) rotation defined by its Lie algebra</span>
<span class="sd">          which is a 2x2 so(2) matrix (skew symmetric)</span>

<span class="sd">        :seealso: :func:`spatialmath.base.transforms2d.trexp`, :func:`spatialmath.base.transformsNd.skew`</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span><span class="mi">2</span><span class="p">))</span> <span class="ow">and</span> <span class="ow">not</span> <span class="n">so2</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">cls</span><span class="p">([</span><span class="n">tr</span><span class="o">.</span><span class="n">trexp2</span><span class="p">(</span><span class="n">s</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="n">check</span><span class="p">)</span> <span class="k">for</span> <span class="n">s</span> <span class="ow">in</span> <span class="n">S</span><span class="p">])</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">cls</span><span class="p">(</span><span class="n">tr</span><span class="o">.</span><span class="n">trexp2</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="n">check</span><span class="p">),</span> <span class="n">check</span><span class="o">=</span><span class="kc">False</span><span class="p">)</span></div>

<div class="viewcode-block" id="SO2.isvalid"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SO2.isvalid">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">isvalid</span><span class="p">(</span><span class="n">x</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Test if matrix is valid SO(2)</span>

<span class="sd">        :param x: matrix to test</span>
<span class="sd">        :type x: numpy.ndarray</span>
<span class="sd">        :return: True if the matrix is a valid element of SO(2), ie. it is a 2x2</span>
<span class="sd">            orthonormal matrix with determinant of +1.</span>
<span class="sd">        :rtype: bool</span>

<span class="sd">        :seealso: :func:`~spatialmath.base.transform3d.isrot`</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">tr</span><span class="o">.</span><span class="n">isrot2</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">True</span><span class="p">)</span></div>

<div class="viewcode-block" id="SO2.inv"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SO2.inv">[docs]</a>    <span class="k">def</span> <span class="nf">inv</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Inverse of SO(2)</span>

<span class="sd">        :return: inverse rotation</span>
<span class="sd">        :rtype: SO2 instance</span>

<span class="sd">        - ``x.inv()`` is the inverse of `x`.</span>
<span class="sd">        </span>
<span class="sd">        Notes:</span>
<span class="sd">            </span>
<span class="sd">            - for elements of SO(2) this is the transpose.</span>
<span class="sd">            - if `x` contains a sequence, returns an `SO2` with a sequence of inverses</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="bp">self</span><span class="p">)</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
            <span class="k">return</span> <span class="n">SO2</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">A</span><span class="o">.</span><span class="n">T</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="n">SO2</span><span class="p">([</span><span class="n">x</span><span class="o">.</span><span class="n">T</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="bp">self</span><span class="o">.</span><span class="n">A</span><span class="p">])</span></div>

    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">R</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        SO(2) or SE(2) as rotation matrix</span>

<span class="sd">        :return: rotational component</span>
<span class="sd">        :rtype: numpy.ndarray, shape=(2,2)</span>

<span class="sd">        ``x.R`` returns the rotation matrix, when `x` is `SO2` or `SE2`. If `len(x)` is:</span>
<span class="sd">            </span>
<span class="sd">        - 1, return an ndarray with shape=(2,2)</span>
<span class="sd">        - N&gt;1, return ndarray with shape=(N,2,2)</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">A</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]</span>

<div class="viewcode-block" id="SO2.theta"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SO2.theta">[docs]</a>    <span class="k">def</span> <span class="nf">theta</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">units</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        SO(2) as a rotation angle</span>
<span class="sd">        </span>
<span class="sd">        :param unit: angular units &#39;deg&#39; or &#39;rad&#39; [default]</span>
<span class="sd">        :type unit: str, optional</span>
<span class="sd">        :return: rotation angle</span>
<span class="sd">        :rtype: float or list</span>
<span class="sd">        </span>
<span class="sd">        ``x.theta`` is the rotation angle such that `x` is `SO2(x.theta)`.</span>
<span class="sd">        </span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="n">units</span> <span class="o">==</span> <span class="s1">&#39;deg&#39;</span><span class="p">:</span>
            <span class="n">conv</span> <span class="o">=</span> <span class="mf">180.0</span> <span class="o">/</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">conv</span> <span class="o">=</span> <span class="mf">1.0</span>
            
        <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="bp">self</span><span class="p">)</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
            <span class="k">return</span> <span class="n">conv</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">A</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span><span class="mi">0</span><span class="p">],</span> <span class="bp">self</span><span class="o">.</span><span class="n">A</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">])</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="p">[</span><span class="n">conv</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">x</span><span class="o">.</span><span class="n">A</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span><span class="mi">0</span><span class="p">],</span> <span class="n">x</span><span class="o">.</span><span class="n">A</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">])</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="bp">self</span><span class="p">]</span></div>
    
<div class="viewcode-block" id="SO2.SE2"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SO2.SE2">[docs]</a>    <span class="k">def</span> <span class="nf">SE2</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Create SE(2) from SO(2)</span>
<span class="sd">        </span>
<span class="sd">        :return: SE(2) with same rotation but zero translation</span>
<span class="sd">        :rtype: SE2 instance</span>

<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">SE2</span><span class="p">(</span><span class="n">tr</span><span class="o">.</span><span class="n">rt2tr</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">A</span><span class="p">,</span> <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]))</span></div></div>

    

<span class="c1"># ============================== SE2 =====================================#</span>

<div class="viewcode-block" id="SE2"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SE2">[docs]</a><span class="k">class</span> <span class="nc">SE2</span><span class="p">(</span><span class="n">SO2</span><span class="p">):</span>
    <span class="c1"># constructor needs to take ndarray -&gt; SO2, or list of ndarray -&gt; SO2</span>
<div class="viewcode-block" id="SE2.__init__"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SE2.__init__">[docs]</a>    <span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">theta</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="o">*</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">True</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Construct new SE(2) object</span>

<span class="sd">        :param unit: angular units &#39;deg&#39; or &#39;rad&#39; [default] if applicable</span>
<span class="sd">        :type unit: str, optional</span>
<span class="sd">        :param check: check for valid SE(2) elements if applicable, default to True</span>
<span class="sd">        :type check: bool</span>
<span class="sd">        :return: homogeneous rigid-body transformation matrix</span>
<span class="sd">        :rtype: SE2 instance</span>
<span class="sd">        </span>
<span class="sd">        - ``SE2()`` is an SE2 instance representing a null motion -- the identity matrix</span>
<span class="sd">        - ``SE2(x, y)`` is an SE2 instance representing a pure translation of (``x``, ``y``)</span>
<span class="sd">        - ``SE2(t)`` is an SE2 instance representing a pure translation of (``x``, ``y``) where``t``=[x,y] is a 2-element array_like</span>
<span class="sd">        - ``SE2(x, y, theta)`` is an SE2 instance representing a translation of (``x``, ``y``) and a rotation of ``theta`` radians</span>
<span class="sd">        - ``SE2(x, y, theta, unit=&#39;deg&#39;)`` is an SE2 instance representing a translation of (``x``, ``y``) and a rotation of ``theta`` degrees</span>
<span class="sd">        - ``SE2(t)`` is an SE2 instance representing a translation of (``x``, ``y``) and a rotation of ``theta`` where ``t``=[x,y,theta] is a 3-element array_like</span>
<span class="sd">        - ``SE2(T)`` is an SE2 instance with rigid-body motion described by the SE(2) matrix T which is a 3x3 numpy array.  If ``check``</span>
<span class="sd">          is ``True`` check the matrix belongs to SE(2).</span>
<span class="sd">        - ``SE2([T1, T2, ... TN])`` is an SE2 instance containing a sequence of N rigid-body motions, each described by an SE(2) matrix</span>
<span class="sd">          Ti which is a 3x3 numpy array. If ``check`` is ``True`` then check each matrix belongs to SE(2).</span>
<span class="sd">        - ``SE2([X1, X2, ... XN])`` is an SE2 instance containing a sequence of N rigid-body motions, where each Xi is an SE2 instance.</span>
<span class="sd">        </span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="nb">super</span><span class="p">()</span><span class="o">.</span><span class="fm">__init__</span><span class="p">()</span>  <span class="c1"># activate the UserList semantics</span>

        <span class="k">if</span> <span class="n">x</span> <span class="ow">is</span> <span class="kc">None</span> <span class="ow">and</span> <span class="n">y</span> <span class="ow">is</span> <span class="kc">None</span> <span class="ow">and</span> <span class="n">theta</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
            <span class="c1"># SE2()</span>
            <span class="c1"># empty constructor</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">data</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)]</span>

        <span class="k">elif</span> <span class="n">x</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
            <span class="k">if</span> <span class="n">y</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span> <span class="ow">and</span> <span class="n">theta</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
                <span class="c1"># SE2(x, y)</span>
                <span class="bp">self</span><span class="o">.</span><span class="n">data</span> <span class="o">=</span> <span class="p">[</span><span class="n">tr</span><span class="o">.</span><span class="n">transl2</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)]</span>
            <span class="k">elif</span> <span class="n">y</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span> <span class="ow">and</span> <span class="n">theta</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
                <span class="c1"># SE2(x, y, theta)</span>
                <span class="bp">self</span><span class="o">.</span><span class="n">data</span> <span class="o">=</span> <span class="p">[</span><span class="n">tr</span><span class="o">.</span><span class="n">trot2</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">t</span><span class="o">=</span><span class="p">[</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">],</span> <span class="n">unit</span><span class="o">=</span><span class="n">unit</span><span class="p">)]</span>
            <span class="k">elif</span> <span class="n">y</span> <span class="ow">is</span> <span class="kc">None</span> <span class="ow">and</span> <span class="n">theta</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
                <span class="k">if</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="mi">2</span><span class="p">):</span>
                    <span class="c1"># SE2([x,y])</span>
                    <span class="bp">self</span><span class="o">.</span><span class="n">data</span> <span class="o">=</span> <span class="p">[</span><span class="n">tr</span><span class="o">.</span><span class="n">transl2</span><span class="p">(</span><span class="n">x</span><span class="p">)]</span>
                <span class="k">elif</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="mi">3</span><span class="p">):</span>
                    <span class="c1"># SE2([x,y,theta])</span>
                    <span class="bp">self</span><span class="o">.</span><span class="n">data</span> <span class="o">=</span> <span class="p">[</span><span class="n">tr</span><span class="o">.</span><span class="n">trot2</span><span class="p">(</span><span class="n">x</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="n">t</span><span class="o">=</span><span class="n">x</span><span class="p">[:</span><span class="mi">2</span><span class="p">],</span> <span class="n">unit</span><span class="o">=</span><span class="n">unit</span><span class="p">)]</span>
                <span class="k">else</span><span class="p">:</span>
                    <span class="nb">super</span><span class="p">()</span><span class="o">.</span><span class="n">_arghandler</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="n">check</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;bad arguments to constructor&#39;</span><span class="p">)</span></div>


<div class="viewcode-block" id="SE2.Rand"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SE2.Rand">[docs]</a>    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">Rand</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="o">*</span><span class="p">,</span> <span class="n">xrange</span><span class="o">=</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">yrange</span><span class="o">=</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">trange</span><span class="o">=</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">2</span> <span class="o">*</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span><span class="p">],</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">,</span> <span class="n">N</span><span class="o">=</span><span class="mi">1</span><span class="p">):</span>
        <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Construct a new random SE(2)</span>
<span class="sd">    </span>
<span class="sd">        :param xrange: x-axis range [min,max], defaults to [-1, 1]</span>
<span class="sd">        :type xrange: 2-element sequence, optional</span>
<span class="sd">        :param yrange: y-axis range [min,max], defaults to [-1, 1]</span>
<span class="sd">        :type yrange: 2-element sequence, optional</span>
<span class="sd">        :param trange: theta range [min,max], defaults to :math:`[0, 2\pi)`</span>
<span class="sd">        :type yrange: 2-element sequence, optional</span>
<span class="sd">        :param N: number of random rotations, defaults to 1</span>
<span class="sd">        :type N: int</span>
<span class="sd">        :return: homogeneous rigid-body transformation matrix</span>
<span class="sd">        :rtype: SE2 instance</span>
<span class="sd">    </span>
<span class="sd">        Return an SE2 instance with random rotation and translation.</span>

<span class="sd">        - ``SE2.Rand()`` is a random SE(2) rotation.</span>
<span class="sd">        - ``SE2.Rand(N)`` is an SE2 object containing a sequence of N random</span>
<span class="sd">          poses.</span>
<span class="sd">        </span>
<span class="sd">        Example, create random ten vehicles in the xy-plane::</span>
<span class="sd">            </span>
<span class="sd">            &gt;&gt;&gt; x = SE3.Rand(N=10, xrange=[-2,2], yrange=[-2,2])</span>
<span class="sd">            &gt;&gt;&gt; len(x)</span>
<span class="sd">            10</span>
<span class="sd">    </span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">x</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">uniform</span><span class="p">(</span><span class="n">low</span><span class="o">=</span><span class="n">xrange</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">high</span><span class="o">=</span><span class="n">xrange</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">size</span><span class="o">=</span><span class="n">N</span><span class="p">)</span>  <span class="c1"># random values in the range</span>
        <span class="n">y</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">uniform</span><span class="p">(</span><span class="n">low</span><span class="o">=</span><span class="n">yrange</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">high</span><span class="o">=</span><span class="n">yrange</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">size</span><span class="o">=</span><span class="n">N</span><span class="p">)</span>  <span class="c1"># random values in the range</span>
        <span class="n">theta</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">uniform</span><span class="p">(</span><span class="n">low</span><span class="o">=</span><span class="n">trange</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">high</span><span class="o">=</span><span class="n">trange</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">size</span><span class="o">=</span><span class="n">N</span><span class="p">)</span>  <span class="c1"># random values in the range</span>
        <span class="k">return</span> <span class="bp">cls</span><span class="p">([</span><span class="n">tr</span><span class="o">.</span><span class="n">trot2</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="n">t</span><span class="o">=</span><span class="p">[</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">])</span> <span class="k">for</span> <span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span> <span class="ow">in</span> <span class="nb">zip</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">,</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getunit</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="p">))])</span></div>

<div class="viewcode-block" id="SE2.Exp"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SE2.Exp">[docs]</a>    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">Exp</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">S</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">True</span><span class="p">,</span> <span class="n">se2</span><span class="o">=</span><span class="kc">True</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Construct a new SE(2) from se(2) Lie algebra</span>

<span class="sd">        :param S: element of Lie algebra se(2)</span>
<span class="sd">        :type S: numpy ndarray</span>
<span class="sd">        :param check: check that passed matrix is valid se(2), default True</span>
<span class="sd">        :type check: bool</span>
<span class="sd">        :param se2: input is an se(2) matrix (default True)</span>
<span class="sd">        :type se2: bool</span>
<span class="sd">        :return: homogeneous transform matrix</span>
<span class="sd">        :rtype: SE2 instance</span>

<span class="sd">        - ``SE2.Exp(S)`` is an SE(2) rotation defined by its Lie algebra</span>
<span class="sd">          which is a 3x3 se(2) matrix (skew symmetric)</span>
<span class="sd">        - ``SE2.Exp(t)`` is an SE(2) rotation defined by a 3-element twist</span>
<span class="sd">          vector array_like (the unique elements of the se(2) skew-symmetric matrix)</span>
<span class="sd">        - ``SE2.Exp(T)`` is a sequence of SE(2) rigid-body motions defined by an Nx3 matrix of twist vectors, one per row.</span>
<span class="sd">          </span>
<span class="sd">        Note:</span>
<span class="sd">            </span>
<span class="sd">        - an input 3x3 matrix is ambiguous, it could be the first or third case above. In this case the argument ``se2`` is the decider.</span>

<span class="sd">        :seealso: :func:`spatialmath.base.transforms2d.trexp`, :func:`spatialmath.base.transformsNd.skew`</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">)</span> <span class="ow">and</span> <span class="n">S</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">==</span> <span class="mi">3</span> <span class="ow">and</span> <span class="ow">not</span> <span class="n">se2</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">cls</span><span class="p">([</span><span class="n">tr</span><span class="o">.</span><span class="n">trexp2</span><span class="p">(</span><span class="n">s</span><span class="p">)</span> <span class="k">for</span> <span class="n">s</span> <span class="ow">in</span> <span class="n">S</span><span class="p">])</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">cls</span><span class="p">(</span><span class="n">tr</span><span class="o">.</span><span class="n">trexp2</span><span class="p">(</span><span class="n">S</span><span class="p">),</span> <span class="n">check</span><span class="o">=</span><span class="kc">False</span><span class="p">)</span></div>
        
<div class="viewcode-block" id="SE2.isvalid"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SE2.isvalid">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">isvalid</span><span class="p">(</span><span class="n">x</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Test if matrix is valid SE(2)</span>

<span class="sd">        :param x: matrix to test</span>
<span class="sd">        :type x: numpy.ndarray</span>
<span class="sd">        :return: true if the matrix is a valid element of SE(2), ie. it is a</span>
<span class="sd">                 3x3 homogeneous rigid-body transformation matrix.</span>
<span class="sd">        :rtype: bool</span>

<span class="sd">        :seealso: :func:`~spatialmath.base.transform2d.ishom`</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">tr</span><span class="o">.</span><span class="n">ishom2</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">True</span><span class="p">)</span></div>

    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">t</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Translational component of SE(2)</span>

<span class="sd">        :param self: SE(2)</span>
<span class="sd">        :type self: SE2 instance</span>
<span class="sd">        :return: translational component</span>
<span class="sd">        :rtype: numpy.ndarray</span>

<span class="sd">        ``x.t`` is the translational vector component.  If ``len(x)`` is:</span>

<span class="sd">        - 1, return an ndarray with shape=(2,)</span>
<span class="sd">        - N&gt;1, return an ndarray with shape=(N,2)</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="bp">self</span><span class="p">)</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">A</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="n">x</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="bp">self</span><span class="o">.</span><span class="n">A</span><span class="p">])</span>
    
<div class="viewcode-block" id="SE2.xyt"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SE2.xyt">[docs]</a>    <span class="k">def</span> <span class="nf">xyt</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        SE(2) as a configuration vector</span>
<span class="sd">        </span>
<span class="sd">        :return: An array :math:`[x, y, \theta]`</span>
<span class="sd">        :rtype: numpy.ndarray</span>
<span class="sd">        </span>
<span class="sd">        ``x.xyt`` is the rigidbody motion in minimal form as a translation and rotation expressed </span>
<span class="sd">        in vector form as :math:`[x, y, \theta]`.  If ``len(x)`` is:</span>
<span class="sd">            </span>
<span class="sd">        - 1, return an ndarray with shape=(3,)</span>
<span class="sd">        - N&gt;1, return an ndarray with shape=(N,3)</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="bp">self</span><span class="p">)</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">r_</span><span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">t</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">theta</span><span class="p">()]</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">r_</span><span class="p">[</span><span class="n">x</span><span class="o">.</span><span class="n">t</span><span class="p">,</span> <span class="n">x</span><span class="o">.</span><span class="n">theta</span><span class="p">()]</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="bp">self</span><span class="p">]</span></div>

<div class="viewcode-block" id="SE2.inv"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SE2.inv">[docs]</a>    <span class="k">def</span> <span class="nf">inv</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Inverse of SE(2)</span>

<span class="sd">        :param self: pose</span>
<span class="sd">        :type self: SE2 instance</span>
<span class="sd">        :return: inverse</span>
<span class="sd">        :rtype: SE2</span>

<span class="sd">        Notes:</span>
<span class="sd">            </span>
<span class="sd">            - for elements of SE(2) this takes into account the matrix structure :math:`T^{-1} = \left[ \begin{array}{cc} R &amp; t \\ 0 &amp; 1 \end{array} \right], T^{-1} = \left[ \begin{array}{cc} R^T &amp; -R^T t \\ 0 &amp; 1 \end{array} \right]`</span>
<span class="sd">            - if `x` contains a sequence, returns an `SE2` with a sequence of inverses</span>

<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="bp">self</span><span class="p">)</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
            <span class="k">return</span> <span class="n">SE2</span><span class="p">(</span><span class="n">tr</span><span class="o">.</span><span class="n">rt2tr</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">R</span><span class="o">.</span><span class="n">T</span><span class="p">,</span> <span class="o">-</span><span class="bp">self</span><span class="o">.</span><span class="n">R</span><span class="o">.</span><span class="n">T</span> <span class="o">@</span> <span class="bp">self</span><span class="o">.</span><span class="n">t</span><span class="p">))</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="n">SE2</span><span class="p">([</span><span class="n">tr</span><span class="o">.</span><span class="n">rt2tr</span><span class="p">(</span><span class="n">x</span><span class="o">.</span><span class="n">R</span><span class="o">.</span><span class="n">T</span><span class="p">,</span> <span class="o">-</span><span class="n">x</span><span class="o">.</span><span class="n">R</span><span class="o">.</span><span class="n">T</span> <span class="o">@</span> <span class="n">x</span><span class="o">.</span><span class="n">t</span><span class="p">)</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="bp">self</span><span class="p">])</span></div>
        
<div class="viewcode-block" id="SE2.SE3"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.pose2d.SE2.SE3">[docs]</a>    <span class="k">def</span> <span class="nf">SE3</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">z</span><span class="o">=</span><span class="mi">0</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Create SE(3) from SE(2)</span>
<span class="sd">        </span>
<span class="sd">        :param z: default z coordinate, defaults to 0</span>
<span class="sd">        :type z: float</span>
<span class="sd">        :return: SE(2) with same rotation but zero translation</span>
<span class="sd">        :rtype: SE2 instance</span>
<span class="sd">        </span>
<span class="sd">        &quot;Lifts&quot; 2D rigid-body motion to 3D, rotation in the xy-plane (about the z-axis) and</span>
<span class="sd">        z-coordinate is settable.</span>

<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">def</span> <span class="nf">lift3</span><span class="p">(</span><span class="n">x</span><span class="p">):</span>
            <span class="n">y</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">4</span><span class="p">)</span>
            <span class="n">y</span><span class="p">[:</span><span class="mi">2</span><span class="p">,:</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">x</span><span class="o">.</span><span class="n">A</span><span class="p">[:</span><span class="mi">2</span><span class="p">,:</span><span class="mi">2</span><span class="p">]</span>
            <span class="n">y</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span><span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="n">x</span><span class="o">.</span><span class="n">A</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span><span class="mi">2</span><span class="p">]</span>
            <span class="n">y</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span><span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="n">z</span>
            <span class="k">return</span> <span class="n">y</span>
        <span class="k">return</span> <span class="n">p3</span><span class="o">.</span><span class="n">SE3</span><span class="p">([</span><span class="n">lift3</span><span class="p">(</span><span class="n">x</span><span class="p">)</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="bp">self</span><span class="p">])</span></div></div>
    

<span class="k">if</span> <span class="vm">__name__</span> <span class="o">==</span> <span class="s1">&#39;__main__&#39;</span><span class="p">:</span>  <span class="c1"># pragma: no cover</span>

    <span class="kn">import</span> <span class="nn">pathlib</span>
    <span class="kn">import</span> <span class="nn">os.path</span>

    <span class="n">exec</span><span class="p">(</span><span class="nb">open</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">pathlib</span><span class="o">.</span><span class="n">Path</span><span class="p">(</span><span class="vm">__file__</span><span class="p">)</span><span class="o">.</span><span class="n">parent</span><span class="o">.</span><span class="n">absolute</span><span class="p">(),</span> <span class="s2">&quot;test_pose2d.py&quot;</span><span class="p">))</span><span class="o">.</span><span class="n">read</span><span class="p">())</span>
    

    
</pre></div>

          </div>
          
        </div>
      </div>
      <div class="sphinxsidebar" role="navigation" aria-label="main navigation">
        <div class="sphinxsidebarwrapper">
<p class="logo">
  <a href="../../index.html">
    <img class="logo" src="../../_static/../../../../figs/icon.png" alt="Logo"/>
    
  </a>
</p>



<p class="blurb">Spatial maths and geometry for Python</p>




<p>
<iframe src="https://ghbtns.com/github-btn.html?user=petercorke&repo=spatialmath-python&type=watch&count=true&size=large&v=2"
  allowtransparency="true" frameborder="0" scrolling="0" width="200px" height="35px"></iframe>
</p>





<h3>Navigation</h3>
<ul>
<li class="toctree-l1"><a class="reference internal" href="../../intro.html">Introduction</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../spatialmath.html">Classes and functions</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../indices.html">Indices</a></li>
</ul>

<div class="relations">
<h3>Related Topics</h3>
<ul>
  <li><a href="../../index.html">Documentation overview</a><ul>
  <li><a href="../index.html">Module code</a><ul>
  </ul></li>
  </ul></li>
</ul>
</div>
<div id="searchbox" style="display: none" role="search">
  <h3 id="searchlabel">Quick search</h3>
    <div class="searchformwrapper">
    <form class="search" action="../../search.html" method="get">
      <input type="text" name="q" aria-labelledby="searchlabel" />
      <input type="submit" value="Go" />
    </form>
    </div>
</div>
<script>$('#searchbox').show(0);</script>








        </div>
      </div>
      <div class="clearer"></div>
    </div>
    <div class="footer">
      &copy;2020, Peter Corke.
      
      |
      Powered by <a href="http://sphinx-doc.org/">Sphinx 2.4.4</a>
      &amp; <a href="https://github.com/bitprophet/alabaster">Alabaster 0.7.12</a>
      
    </div>

    

    
  </body>
</html>